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Abstract 

Stable estimation of rigid body pose and velocities from noisy measurements, without any knowledge of the dynamics model, 
is treated using the Lagrange-d’Alembert principle from variational mechanics. With body-fixed optical and inertial sensor 
measurements, a Lagrangian is obtained as the difference between a kinetic energy-like term that is quadratic in velocity 
estimation error and the sum of two artificial potential functions; one obtained from a generalization of Wahba’s function for 
attitude estimation and another which is quadratic in the position estimate error. An additional dissipation term that is linear 
in the velocity estimation error is introduced, and the Lagrange-d’Alembert principle is applied to the Lagrangian with this 
dissipation. A Lyapunov analysis shows that the state estimation scheme so obtained provides stable asymptotic convergence 
of state estimates to actual states in the absence of measurement noise, with an almost global domain of attraction. This 
estimation scheme is discretized for computer implementation using discrete variational mechanics, as a hrst order Lie group 
variational integrator. The continuous and discrete pose estimation schemes require optical measurements of at least three 
inertially fixed landmarks or beacons in order to estimate instantaneous pose. The discrete estimation scheme can also estimate 
velocities from such optical measurements. Moreover, all states can be estimated during time periods when measurements of 
only two inertial vectors, the angular velocity vector, and one feature point position vector are available in body frame. In the 
presence of bounded measurement noise in the vector measurements, numerical simulations show that the estimated states 
converge to a bounded neighborhood of the actual states. 


1 Introduction 

Estimation of rigid body translational and rotational 
motion is indispensable for operations of spacecraft, un¬ 
manned aerial and underwater vehicles. Autonomous 
state estimation of a rigid body based on inertial vector 
measurement and visual feedback from stationary land¬ 
marks, in the absence of a dynamics model for the rigid 
body, is analyzed here. The estimation scheme proposed 
here can also be applied to relative state estimation with 
respect to moving objects [l^. This estimation scheme 
can enhance the autonomy and reliability of unmanned 
vehicles in uncertain GPS-denied environments. Salient 
features of this estimation scheme are: (1) use of on¬ 
board optical and inertial sensors, with or without rate 
gyros, for autonomous navigation; (2) robustness to un¬ 
certainties and lack of knowledge of dynamics; (3) low 
computational complexity for easy implementation with 
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onboard processors; (4) proven stability with large do¬ 
main of attraction for state estimation errors; and (5) 
versatile enough to estimate motion with respect to sta¬ 
tionary as well as moving objects. Robust state estima¬ 
tion of rigid bodies in the absence of complete knowledge 
of their dynamics, is required for their safe, reliable, and 
autonomous operations in poorly known conditions. In 
practice, the dynamics of a vehicle may not be perfectly 
known, especially when the vehicle is under the action 
of poorly known forces and moments. The scheme pro¬ 
posed here has a single, stable algorithm for the coupled 
translational and rotational motion of rigid bodies using 
onboard optical (which may include infra-red) and iner¬ 
tial sensors. This avoids the need for measurements from 
external sources, like GPS, which may not be available in 
indoor, underwater or cluttered environments P- flTLI^ . 

Attitude estimators using unit quaternions for attitude 
representation may be unstable in the sense of Lyapunov, 
unless they identify antipodal quaternions with a sin¬ 
gle attitude. This is also the case for attitude control 
schemes based on continuous feedback of unit quater¬ 
nions, as shown in sam. One adverse consequence 
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of these unstable estimation and control schemes is that 
they end up taking longer to converge compared with 
stable schemes under similar initial conditions and initial 
transient behavior. Continuous-time attitude observers 
and filtering schemes on SO0) and SE(3) have been re¬ 
ported in, e.g., [l,[I3,[il,[il,l3[S3i[13i[20,[H)[13- These 
estimators do not suffer from kinematic singularities like 
estimators using coordinate descriptions of attitude, and 
they do not suffer from unwinding as they do not use 
unit quaternions. The maximum-likelihood (minimum 
energy) filtering method of Mortensen [2^ was recently 
applied to attitude estimation, resulting in a nonlin¬ 
ear attitude estimation scheme that seeks to minimize 
the stored “energy” in measurement errors cmaiig. 
This scheme is obtained by applying Hamilton-Jacobi- 
Bellman (HJB) theory [T^ to the state space of attitude 
motion [^ . Since the HJB equation can only be ap¬ 
proximately solved with increasingly unwieldy expres¬ 
sions for higher order approximations, the resulting fil¬ 
ter is only “near optimal” up to second order. Unlike fil¬ 
tering schemes that are based on approximate or “near 
optimal” solutions of the HJB equation and do not have 
provable stability, the estimation scheme obtained here 
can be solved exactly, and is shown to be almost glob¬ 
ally asymptotically stable. Moreover, unlike filters based 
on Kalman filtering, the estimator proposed here does 
not presume any knowledge of the statistics of the ini¬ 
tial state estimate or the sensor noise. Indeed, for vector 
measurements using optical sensors with limited field-of- 
view, the probability distribution of measurement noise 
needs to have compact support, unlike standard Gaus¬ 
sian noise processes that are commonly used to describe 
such noisy measurements. 

The variational attitude estimator recently appeared in 
EElIil, where it was shown to be almost globally 
asymptotically stable. Some of the advantages of this 
scheme over some commonly used competing schemes 
are reported in Q. This paper is the variational esti¬ 
mation framework to coupled rotational (attitude) and 
translational motion, as exhibited by maneuvering vehi¬ 
cles like UAVs. In such applications, designing separate 
state estimators for the translational and rotational mo¬ 
tions may not be effective and may lead to poor naviga¬ 
tion. For navigation and tracking the motion of such ve¬ 
hicles, the approach proposed here for robust and stable 
estimation of the coupled translational and rotational 
motion will be more effective than de-coupled estimation 
of translational and rotational motion states. Moreover, 
like other vision-inertial navigation schemes [H, , the 

estimation scheme proposed here does not rely on GPS. 
However, unlike many other vision-inertial estimation 
schemes, the estimation scheme proposed here can be 
implemented without any direct velocity measurements. 
Since rate gyros are usually corrupted by high noise 
content and bias such a velocity measurement-free 
scheme can result in fault tolerance in the case of faults 
with rate gyros. Additionally, this estimation scheme can 
be extended to relative pose estimation between vehicles 


from optical measurements, without direct communica¬ 
tions or measurements of relative velocities. 

The contents of this article are organized as follows. In 
Section 2, the problem of motion estimation of a rigid 
body using onboard optical and inertial sensors is intro¬ 
duced. The measurement model is introduced and rigid 
body states are related to these measurements. Section 3 
introduces artificial energy terms representing the mea¬ 
surement residuals corresponding to the rigid body state 
estimates. The Lagrange-d’Alembert principle is applied 
to the Lagrangian constructed from these energy terms 
with a Rayleigh dissipation term linear in the velocity 
measurement residual, to give the continuous time state 
estimator. Particular versions of this estimation scheme 
are provided for the cases when direct velocity measure¬ 
ments are not available and when only angular velocity 
is directly measured. Section 4 proves the stability of 
the resulting variational estimator. It is shown that, in 
the absence of measurement noise, state estimates con¬ 
verge to actual states asymptotically and the domain of 
attraction is an open dense subset of the state space. In 
Section 5, the variational pose estimator is discretized as 
a Lie group variational integrator, by applying the dis¬ 
crete Lagrange-d’Alembert principle to discretizations 
of the Lagrangian and the dissipation term. This estima¬ 
tor is simulated numerically in Section 6, for two cases: 
the case where at least three beacons are measured at 
each time instant; and the under-determined case, where 
occasionally less than three beacons are observed. For 
these simulations, true states of an aerial vehicle are gen¬ 
erated using a given dynamics model. Optical/inertial 
measurements are generated, assuming bounded noise 
in sensor readings. Using these measurements, state es¬ 
timates are shown to converge to a neighborhood of ac¬ 
tual states, for both cases simulated. Finally, Section 7 
lists the contributions and possible future extensions of 
the work presented in this paper. 


2 Navigation using Optical and Inertial Sensors 

Consider a vehicle in spatial (rotational and transla¬ 
tional) motion. Onboard estimation of the pose of the 
vehicle involves assigning a coordinate frame fixed to the 
vehicle body, and another coordinate frame fixed in the 
environment which takes the role of the inertial frame. 
Let O denote the observed environment and S denote 
the vehicle. Let S denote a coordinate frame fixed to S 
and 0 be a coordinate frame fixed to O, as shown in 
Fig. 1. Let R G SO(3) denote the rotation matrix from 
frame S to frame 0 and b denote the position of origin of 
S expressed in frame 0. The pose (transformation) from 
body fixed frame S to inertial frame 0 is then given by 
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Fig. 1. Inertial landmarks on O as observed from vehicle S 
with optical measurements. 

Consider vectors known in inertial frame 0 measured by 
inertial sensors in the vehicle-fixed frame S; let /3 be the 
number of such vectors. In addition, consider position 
vectors of a few stationary points in the inertial frame 
0 measured by optical (vision or lidar) sensors in the 
vehicle-fixed frame S. Velocities of the vehicle may be 
directly measured or can be estimated by linear filter¬ 
ing of the optical position vector measurements [l^. As¬ 
sume that these optical measurements are available for 
j points at time t, whose positions are known in frame 
0 as pj, j G I(t), where I{t) denotes the index set of 
beacons observed at time t. Note that the observed sta¬ 
tionary beacons or landmarks may vary over time due to 
the vehicle’s motion. These points generate (^) unique 
relative position vectors, which are the vectors connect¬ 
ing any two of these landmarks. When two or more posi¬ 
tion vectors are optically measured, the number of vec¬ 
tor measurements that can be used to estimate attitude 
is (^) + p. This number needs to be at least two (i.e., 
( 2 ) +/3 ^ 2) at an instant, for the attitude to be uniquely 
determined at that instant. In other words, if at least two 
inertial vectors are measured at all instants (i.e., /3 > 2), 
then beacon position measurements are not required for 
estimating attitude. However, at least one beacon or fea¬ 
ture point position measurement is still required to es¬ 
timate the position of the vehicle. Note that the use of 
two vector measurements for attitude determination was 
first proposed by the TRIAD algorithm in the 1960s Q. 

2.1 Pose Measurement Model 

Denote the position of an optical sensor and the unit 
vector from that sensor to an observed beacon in frame 
S as G and G S^, k = 1,..., ^, respectively. 
Denote the relative position of the stationary beacon 
observed by the sensor expressed in frame S as qj’. 


Thus, in the absence of measurement noise 

Pj = R{Qj T b = Roj -|- 6, J G 2i(t), (2) 

where Oj = + s^, are positions of these points ex¬ 

pressed in S. In practice, the Oj are obtained from range 
measurements that have additive noise; we denote as a™ 
the measured vectors. In the case of lidar range measure¬ 
ments, these are given by 

af = [q^r + s" = ig’;ru^ + j g m, (s) 

where (qj’)’" is the measured range to the point by the 

kth 

sensor. The mean of the vectors pj and a™ are de¬ 
noted as p and a™ respectively, and satisfy 

= ( 4 ) 

J j 

where p = i ^ additive 

J J J 

measurement noise obtained by averaging the measure¬ 
ment noise vectors for each of the Oj. Consider the ( 2 ) 
relative position vectors from optical measurements, de¬ 
noted as dj = p\ — pe in frame 0 and the correspond¬ 
ing vectors in frame S as Ij = a\ — at, for X,i G T(t), 
\ ^ i. The P measured inertial vectors are included in 
the set of dj, and their corresponding measured values 
expressed in frame S are included in the set of Ij. If the 
total number of measured vectors (both optical and in¬ 
ertial), ii) + P = 2, then = li x I 2 is considered a 
third measured direction in frame S with corresponding 
vector da = di X d 2 in frame 0. Therefore, 

dj = Rlj D = RL, (5) 

where D = [di • • • d„], L = [li • • • /„] G with 

n = 3 if (^) -I- /3 = 2 and n= (^) -|- /3 if (^) -|- /3 > 2. Note 
that the matrix D consists of vectors known in frame 0. 
Denote the measured value of matrix L in the presence 
of measurement noise as L™. Then, 

L™ = R^D + (6) 

where ^ G R^^" consists of the additive noise in the 
vector measurements made in the body frame S. 

2.2 Velocities Measurement Model 

Denote the angular and translational velocity of the rigid 
body expressed in body fixed frame S by D and v, re¬ 
spectively. Therefore, one can write the kinematics of 
the rigid body as 

h = R^l^,b=Rv^g = gC^, (7) 
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where ^ 
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and(-)'' : ^■ 


so(3) C R^x3 jg skew-symmetric cross-product op¬ 
erator that gives the vector space isomorphism between 
R^ and so(3): 


3.1 Lagrangian from Measurement Residuals 


Consider the sum of rotational and translational mea¬ 
surement residuals between the measurements and esti¬ 
mated pose as a potential energy-like function. Defining 
the trace inner product on as 
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( 8 ) 


For the general development of the motion estimation 
scheme, it is assumed that the velocities are directly mea¬ 
sured. The estimator is then extended to cover the cases 
where: (i) only angular velocity is directly measured; and 
(ii) none of the velocities are directly measured. 


3 Dynamic Estimation of Motion from Proxim¬ 
ity Measurements 

In order to obtain state estimation schemes from mea¬ 
surements as outlined in Section 2 in continuous time, 
the Lagrange-d’Alembert principle is applied to an ac¬ 
tion functional of a Lagrangian of the state estimate er¬ 
rors, with a dissipation term linear in the velocities esti¬ 
mate error. This section presents the estimation scheme 
obtained using this approach. Denote the estimated pose 
and its kinematics as 


(Ai, A 2 ) := trace(A^A2), (13) 

the rotational potential function (Wahba’s cost func¬ 
tion [s^]) is expressed as 

U°{g,L^,D) = ^{D-RL^,{D-RL"^)W), (14) 

where W = diag(wj) G R^x" is a positive diagonal 
matrix of weight factors for the measured IJ^. Consider 
the translational potential function 

Ut{g,d^,p) = = ^K\\p-Ra^ - , (15) 

where p is defined by (4), y = y(g, d^,p) = p — RaJ^ — h 
and K is a positive scalar. Therefore, the total potential 
function is defined as the sum of the generalization of 
(14) defined in [T^, for attitude determination on 
SO(3), and the translational energy (15) as 


gSE( 3), i = gf, (9) 

where f is rigid body velocities estimate, with go as the 
initial pose estimate and the pose estimation error as 


R b 
0 1 


h = 


1 _ 

'q 

b-Qb 


Q X 


_0 

I 


0 I_ 


gSE(3), (10) 


^ 'T' . 

where Q = RR^ is the attitude estimation error and 
X = b — Qb. Then one obtains, in the case of perfect 
measurements, 


h = hif', where 


Adg(r-I), 

( 11 ) 


W(g, L™, D, d^,p) = Ur{g, L™, D) + Ut{g, d^,p) 

= 4>(W°(g,L'",D))+ZY*(ia™,p) 

= - RL"^, (D - RL'^)W)) 

+ (16) 

where W is positive definite (not necessarily diagonal), 
and 4) : [0,oo) i-G [0,oo) is a function that satisfies 
4)(0) = 0 and 4>'(;y) > 0 for all ;c G [0, 00 ). Furthermore, 
‘l’^(-) < «(•) where a(-) is a Class-/C function [l^ and 
4)'(-) denotes the derivative of 4>(-) with respect to its 
argument. Because of these properties of the function 4>, 
the critical points and their indices coincide for and 
Ur 0 . Define the kinetic energy-like function: 

r(<^(g,r,o) = ^^(g,r,eyMg,r,e^ (17) 


where Ad_y = 


6x^ 




ioi g = 


■K. 

0 


. The atti¬ 


tude and position estimation error dynamics are also in 
the form 


Q = Qoj^, X = Qv. 


( 12 ) 


where JJ G R®x® > 0 is an artificial inertia-like kernel 
matrix. Note that in contrast to rigid body inertia ma¬ 
trix, J is not subject to intrinsic physical constraints like 
the triangle inequality, which dictates that the sum of 
any two eigenvalues of the inertia matrix has to be larger 
than the third. Instead, J is a gain matrix that can be 
used to tune the estimator. For notational convenience, 
<f{g,£,‘^A) is denoted as (p from now on; this quantity 
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is the velocities estimation error in the absence of mea¬ 
surement noise. Now define the Lagrangian 

£(g, L™, D, a^,p, = r(^) - U{g, L™, D, a^,p), 

( 18 ) 

and the corresponding action functional over an arbi¬ 
trary time interval [to,T] for T > 0, 

S{C{g,L^,D,a^,p,^)) = r £{g,L”^,D,a^,p,^)dt, 

Jto 

such that g = ■ The following statement gives the 

form of the Lagrangian when perfect (noise-free) mea¬ 
surements are available, and derives the variational es¬ 
timator for rigid body pose and velocities. 

Lemma 3.1 In the absence of measurement noise, the 
Lagrangian is of the form 

C{h,D,p,ip) -Q,K)) - ^Ky'^y, 

( 20 ) 

where K = DWD^ andy = y{h,p) = Q'^x + {I — Q'^)P- 

Proof. Suppose that all the measured states are noise 
free. Therefore, one can replace L™ = L, d™ = d and 
rotational potential function (14) can be 

replaced by 

U°{h,D) = ^{D-RL^,{D-RL^)W) 

= ^{D-Q^D,{D-Q^D)W) (21) 

= i(/ - (j _ q^)dwD^) = {I-Q, K), 

/S rp 

since RE = Q D ior the noise-free case. In addition, 

y{h,p) = p — Ra^ — b = p — Rd — b (22) 

= p — Rd — [b — x) = Q^x + {I — Q^)P- 

The translational potential function in the absence of 
measurement noise can be expressed as 

^t(h,p) = ^Ky'l'y. (23) 

Therefore, the total potential energy function is 

U{\^,D,p) = Urlfa,D) +Utlfa,p) 

= ^{U°{h,D)) PUtiKp) 

= ^{{I-Q,K)) p]^Ky^y, (24) 


and the kinetic energy function is 

T{p) = (25) 

Substituting (24) and (25) into: 

C{\\,D,p,ip) = T{ip) -U{\\,D,p) 

= r(^)-<l>(W°(h,ll))-Wt(h,p), (26) 

gives the Lagrangian (20) for the noise-free case. □ 

As in m , the positive definite weight matrix W can be 
selected according to the following lemma: 

Lemma 3.2 Let rank{D) = 3. Let the singular value 
decomposition of D be given by 

D : = Ud^dVu where Ud £ 0(3), Vd £ 0(n), 

Ed £ E)iag+(3,n), (27) 

and Diag~*'(ni, nf) is the vector space ofni x n 2 matrices 
with positive entries along the main diagonal and all other 
components zero. Let cti , (T 2 , era denote the main diagonal 
entries of Ed . Further, let the positive definite weight 
matrix W be given by 

W = VdWqV^ where Wq £ Diag'''(n,n) (28) 

and the first three diagonal entries of Wq are given by 

wi = W 2 = %, W 3 = ^ where gi,g2,‘i3 > 0. 
erf erf erf 

(29) 

rp 

Then, K = DWD^ is positive definite and 

K = UdAC/^ where A = dja 3 (<ji,C 2 ,<^ 3 ), (30) 

is its eigendecomposition. Moreover, if c, ^ for i ^ j 
and i,j£ {1, 2, 3}, then (/ — Q, K) is a Morse function 
whose critical points are 

Q€Cq = {/, Qi, g 2 , ga} where Q, = 2 UdU^U^ - I, 

(31) 

and I I is the column vector of the identity I S SO (3). 
The proof is presented in m- 

3.2 Variational Estimator for Pose and Velocities 

The nonlinear variational estimator obtained by 
applying the Lagrange-d’Alembert principle to the La¬ 
grangian (18) with a dissipation term linear in the 
velocities estimation error, is given by the following 
statement. 
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Theorem 3.3 The nonlinear variational estimator for 
pose and velocities is given by 


fjtp = adptp--Dtp, 

h (32) 

[k =mr, 


where adj = (adc)”^ with ad,^ defined by (36), and 
Z{g,L'^,D,d'^,p) is defined by 

Zig,L^,D,d^,p) = 


^'(u°ig,L^,D)'jSr{R) + np^y 

ny 


(33) 


whereU^[g,L™‘,D) is defined as (14), y = y{g,a^,p) = 
p — RdJ^ — b and 

Sr{R) = vex{TR'^ - RT'^) 

= Yex{DW{L^)'^R'^ - RL'^Wd'^), (34) 


where 

Sk{Q) =^ex{KQ - Q"^K)- (40) 

Taking first variation of the translational potential en¬ 
ergy term (23) with respect to Q and x yields: 

ShUtih^p) = k{6x + 6Qp)"^{x + {Q - I)p} 

= K{p'^y + Z^p^y). (41) 


Therefore, the first variation of the total potential energy 
(24) with respect to estimation errors is 

SuUih,D,p) = z'^{h,D,p)y, (42) 

where Z{h,D,p) is defined by 

Z{h,D,p)= (43) 

((/ - g, if)) Sk{Q) + I^P^ {Q^x + (/ - gT)p} 
k{Q^x + (i - q"^)p} 


r = DW{L'^) ^ and vex(-) : so(3) ^ is the inverse Taking the first variation of the kinetic energy term (25) 
of the (•)^ map. with respect to p results in: 


Proof. A Rayleigh dissipation term linear in the velocities 
of the form Hip where D S > 0 is used in addition 
to the Lagrangian (20), and the Lagrange-d’Alembert 
principle from variational mechanics is applied to ob¬ 
tain the estimator on TSE(3). Reduced variations with 
respect to h and p [EEl are applied, given by 


(5h = hr]'^, S(p = p + adipTi, 


where = 


P 

0 0 


and ad|j = 


w 


(35) 

(36) 


for 77 = 


€ and C = 
p{T) = 0. This leads to the expression: 

F 

/to 


b with yitfi) = 


5u,^S{C{h,D,p,<p)) = f rf^Bipdt. (37) 

Jtn 


Note that the variations of the attitude and position 
estimation errors are of the form 

6Q = QT,^, 6x = Qp, (38) 

respectively. Applying reduced variations to the rota¬ 
tional potential energy term ( 21 ), one obtains 


6QU°{h,D) = {-QE^,K) 

= ^J(g)s, 


i(E^xg-gTAb 

(39) 


S:f,T{p) = + ad^rj), (44) 

applying the reduced variation for 6p as given in (35). 
Therefore, the first variation of the action functional (19) 
is obtained as 


6u,^S{C{h,D,p, p)) 

fT 

= I + ad^ri) - rf^Z{h,D,p)}dt 



ad* - 2'(h, D,p) 
ad*J</ 7 - Z(h,£),p) 


J(^) dt + p^Sp\J^ 

Jy3)dt, (45) 


applying fixed endpoint variations with r/{to) = r](T) = 
0. Substituting (45) in expression (37) one obtains 

J (/3 = ad* JI (/7 — Z)!:, i4,p) — Dtp, (46) 


where 2'(h,i4,p) is defined by (43). In order to imple¬ 
ment this estimator using the aforementioned measure- 
ments, substitute Q D = RU^. This changes the rota¬ 
tional potential energy formed by the estimation errors 
in attitude (21) to (14). Equation (40) is also reformu¬ 
lated as 
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Sk{Q) = Yex{DWD^Q - Q^DWD^) (47) 

= Yex{DW{L'^ f R^ - R{L'^)WD^) = St{R). 














Finally, the second row in the matrix Z(\r\,D,p) is re¬ 
placed by 

k{Q"^x + {I - Q"^)p} = -h + p- Q"^p) 

= K{RR^{h — p) — h + p} 

= K{-Ra^ - h + p}. (48) 

Taking these changes into account, one could obtain the 
first of equations (32) with Z(g, L™, D,a^,p) and Sr{R) 
defined by (33) and (34), respectively. Thus, the com¬ 
plete nonlinear estimator equations are given by (32). □ 

This is a fundamentally new idea of applying a principle 
from variational mechanics to obtain a state estimator, 
recently applied to rigid body attitude estimation in [ig. 
This approach differs from the “minimum-energy” ap¬ 
proach to nonlinear estimation due to Mortensen in 
some important ways. The minimum-energy approach 
applies Hamilton-Jacobi-Belhnan (HJB) theory 0, 
which can only be “approximately solved.” This ap¬ 
proach was recently applied to state estimation of rigid 
body attitude motion in [sg. This HJB formulation 
can only be approximately solved in practice, using a 
Riccati-like equation, to obtain a near-optimal filter 
that has no guarantees on stability. In the proposed 
approach, the time evolution of (g,^) has the form of 
the dynamics of a rigid body with Rayleigh dissipa¬ 
tion. This results in an estimator for the motion states 
(g,5) that dissipates the “energy” content in the esti¬ 
mation errors (h,(/3) = (gg“^,Adg(^ — ^)) to provide 
guaranteed asymptotic stability in the case of perfect 
measurements [13]. The differences between these two 
approaches were detailed in Q, for rigid body attitude 
estimation. 


mass-spring-damper system is a “forced Euler-Poincare 
system” SEl with passive dissipation, as is obtained 
here. Explicit expressions for the vector of velocities 
can be obtained for two common cases when these ve¬ 
locities are not directly measured. These two cases are 
dealt with in the next subsection. 


3.3 Variational Estimator Implemented without Direct 
Velocity Measurements 

The velocity measurements in (32) can be replaced by 
hltered velocity estimates obtained by linear filtering of 
optical and inertial measurements using, e.g., a second- 
order Butterworth filter. This is both useful and neces¬ 
sary when velocities are not directly measured. The fil¬ 
tered values are then used in place of to enhance 
the nonlinear estimator given by Theorem 3.3. Denote 
the measured vector quantity at time t by z™. A linear 
second-order filter of the form: 

+ 2pU}nZ^ = — z^), (49) 

is used, where is the natural (cutoff) frequency, p is 
the damping ratio, and z^ is the filtered value of z^. 
Thereafter, z^ is used in place of z^ in equations (32). 

3.3.1 Angular velocity is measured using rate gyros 

Eor the case that rate gyro measurements of angular ve¬ 
locities are available besides the j feature point (or bea¬ 
con) position measurements, the linear velocities of the 
rigid body can be calculated using each single position 
measurement by rewriting (52) as 


The proposed estimator combines certain desirable 
features of stochastic estimation and observer design 
approaches to state estimation for unmanned vehicles, 
when simultaneous inertial vector measurements and 
optical measurements of fixed beacons or landmarks are 
available. This nonlinear estimator is robust to mea¬ 
surement noise and does not require a dynamics model 
for the vehicle; instead, it estimates the dynamics of the 
vehicle given the measurement model in Section 2. The 
variational pose estimator can also be interpreted as a 
low-pass stable filter (cf. [ig). Indeed, one can connect 
the low-pass filter interpretation to the simple exam¬ 
ple of the natural dynamics of a mass-spring-damper 
system. This is a consequence of the fact that the mass¬ 
spring-damper system is a mechanical system with 
passive dissipation, evolving on a configuration space 
that is the vector space of real numbers, R. In fact, 
the equation of motion of this system can be obtained 
by application of the Lagrange-d’Alembert principle on 
the configuration space R. If this analogy or interpre¬ 
tation is extended to a system evolving on a Lie group 
as a configuration space, then the generalization of the 


z/^ = (aJ)^D^-n/, (50) 


for the point. Averaging the values of i/ derived from 
all feature points gives a more reliable result. Therefore, 
the rigid body’s filtered velocities are expressed in this 
case as 






i=i 





(51) 


3.3.2 Translational and angular velocity measurements 
are not available 

In the case that both angular and translational velocity 
measurements are not available or accurate, rigid body 
velocities can be calculated in terms of the inertial and 
optical measurements. In order to do so, one can differ- 
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entiate (2) as follows 

Pj = + Rdj + i) = R(ri^aj + dj + v') =0 

=^dj — + ly = 0 

=>tij = dj = — I]^ = G{aj)^, (52) 

where G{aj) = — /] has full row rank. From vision- 

based or Doppler lidar sensors, one can also measure 
the velocities of the observed points in frame S, denoted 
f™. Here, velocity measurements as would be obtained 
from vision-based sensors is considered. The measure¬ 
ment model for the velocity is of the form 

= G{aj)^ + ^j, (53) 

where 'dj S is the additive error in velocity measure¬ 
ment u™. Instantaneous angular and translational ve- 
locity determination from such measurements is treated 
in [^. Note that Vj = dj, for j € R{t). As this kinemat¬ 
ics indicates, the relative velocities of at least three bea¬ 
cons are needed to determine the vehicle’s translational 
and angular velocities uniquely at each instant. However, 
when only one or two landmarks/beacons are measured, 
the estimator can propagate velocity estimates based on 
a least squares velocity determined from the available 
measurements. The rigid body velocities in both cases 
are obtained using the pseudo-inverse of (G(A^): 

= Y{V^) ^ = (Et(A^)V(H^), (54) 



G{a() 


f 

Vi 

where G{A^) = 

G{aj)_ 

and Y{V^) = 

f 

Vj 


for G ^{t)- When at least three beacons 

are measured, ©(A-^) is a full column rank ma¬ 
trix, and (Gt(A0 = (G^{Af)G{Af)y^G^{Af) 

gives its pseudo-inverse. For the case that only 
one or two beacons are observed, G(A'^) is a full 
row rank matrix, whose pseudo-inverse is given by 

Gt(A/) = gT(A-^)(g(A/)gT(A^))’\ 

4 Stability and Robustness of Estimator 

The stability of the estimator (filter) given by Theo¬ 
rem 3.3 is analyzed here. The following result shows that 
this scheme is stable, with almost global convergence of 
the estimated states to the real states in the absence of 
measurement noise. 

Theorem 4.1 Let the observed position vectors from 
optical measurements be bounded. Then, the estimator 
presented in Theorem 3.3 is asymptotieally stable at the 
estimation error state (h,(/j) = (/, 0) in the absence of 


measurement noise. Further, the domain of attraction of 
(h, (p) = (/, 0) is a dense open subset o/SE(3) x R®. 

Proof: In the absence of measurement noise, RE = 
Q^D. Therefore, 4>(W0(g,L™,£>)) = $(W°(h,D)) is a 
Morse function on SO (3). The stability of this estimator 
can be shown using the following candidate Morse- 
Lyapunov function, which can be interpreted as the to¬ 
tal energy function (equal in value to the Hamiltonian) 
corresponding to the Lagrangian (18): 

V(h,D,p,ip) =Tiip)+ll{U,D,p) (56) 

= - Q, K)) + ^y^y. 

Note that H(h, D,p, p) >0 and H(h, D,p, (/?) = 0 if and 
only if (h, p) = (I, 0). Therefore, H(h, D,p, p) is positive 
definite on SE(3) x R®. Using (12), one can derive the 
time derivative of (24) as 

^ll{h,D,p) = ‘^>'{Ll°{h,D)){-Qcv^,K) + K{x + Qpf{Qy) 
= $'(W0(h,D))(u;^-gT/^) 

-I- k{Qv + Qoj^p)"^{Qy) 

= ^^’{U^{h,D)){uj^,KQ-Q^K) 

-I- k{v + bJ^p)^y 
= 4>'(W°(h, D))S^{Q)uj + ny^v + «:(p^y)'^w 
= Z^[h,D,p)p, (57) 

where Sk{Q) is defined as (40) and Z{h,D,p) as (43). 
Therefore, the time derivative of the candidate Morse- 
Lyapunov function is 

V{h,D,p,p) = p^Ip + p^Z{h,D,p) 

= T^ydl,Ip-Z(h,D,p)-'S])p + Z{h,D,p)^ 

= -p^np. (58) 

noting that p = 0. Hence, the derivative of the 

Morse-Lyapunov function is negative semi-definite. Note 
that the error dynamics for the pose estimate error h is 
given by (11), while the error dynamics for the veloci¬ 
ties estimate error p is given by (46). Note that D{t), 
as a function of time, is piecewise continuous and uni¬ 
formly bounded. The first property (piecewise continu¬ 
ity) is naturally satisfied by D{t), which is piecewise con¬ 
stant as the number and inertial positions of beacons (or 
feature points) observed by body-fixed optical sensors is 
piecewise continuous in time. The second property (uni¬ 
form boundedness) is satisfied by D{t) if the position vec¬ 
tors observed are bounded in R®, as assumed in the state¬ 
ment. Therefore, the error dynamics for (h,(/j) is non- 
autonomous. Considering (56) and (58), and applying 






Theorem 8.4 in [T^, one can conclude that 0 

as t —>■ oo, which consequently implies (/? —>■ 0. Thus, the 
positive limit set for this system is contained in 

£ = T/-i(0) = {(h,^) e SE(3) X se(3) : ^ = O}. (59) 

Substituting (^ = 0 in the first equation of the estimator 
(32), we obtain the positive limit set where V = 0 (or 
= 0) as the set 

= {(h,(/3) e SE(3) X R® : Z{h,D,p) = 0,(/5 = O} 

(60) 

= {(h,(^)eSE(3)x]R®:QeCQ, ^'^1 = 0, ^ = 0}, 


going backwards in time. This implies that the metric 
distance between error states (h,(/j) along these trajec¬ 
tories on the stable manifolds A4f grows with the time 
separation between these states, and this property does 
not depend on the choice of the metric on SE(3) x R®. 
Therefore, these stable manifolds are embedded (closed) 
submanifolds of SE(3) x R® and so is their union. Clearly, 
all states starting in the complement of this union, con¬ 


verge to the stable equilibrium 




a,0); 


therefore the domain of attraction of this equilibrium is 


DOA{(/, 0)} = SE(3) X R® \ { Mf}, 


where Cq is defined by (31). Therefore, in the absence 
of measurement errors, all the solutions of this estimator 
converge asymptotically to the set y. Define Ur{Q) := 
$((/ — Q, K )), which is the attitude measurement resid¬ 
ual in the case of perfect measurements. Thus, the atti¬ 
tude estimate error converges to the set of critical points 
of Lir{Q) in this intersection, and the position estimate 
error x converges to zero. The unique global minimum 
oiUr{Q) is at (5 = d (Lemma 2.1 in [l3l), so this esti¬ 
mation error is asymptotically stable. 

Now consider the set 


'r=^\(/,0), (61) 


which consists of all stationary states that the estima¬ 
tion errors may converge to, besides the desired esti¬ 
mation error state (4,0). Note that all states in the 
stable manifold of a stationary state in converge to 
this stationary state. Erom the properties of the criti¬ 
cal points Qi e Cq \ (I) of U^{Q), {i= 1, 2, 3) given in 
Lemma 2.1 of [l^, we see that the stationary points in 


^\(/, 0 ) = {( 


, 0) : Qi G Cq \ (/)} have stable 


manifolds whose dimensions depend on the index of Q^. 
Since the velocities estimate error ip converges globally 
to the zero vector, the dimension of the stable manifold 


of the critical points, i.e. ( 


Q. 

0 


,0) e SE(3)xR® 


is 


dim(A4f) = 9 -I- (3 — index of QJ = 12 — index of Q^. 

(62) 


Therefore, the stable manifolds of (h, (^) 




are nine-dimensional, ten-dimensional, or eleven¬ 
dimensional, depending on the index of € Cq \ (/) 
according to (62). Moreover, the value of the Lyapunov 
function V{\\,D,ip) is non-decreasing (increasing when 
(h,(/3) ^ J^) for trajectories on these manifolds when 


which is a dense open subset of SE(3) x R®. □ 

Therefore, the domain of attraction for the variational 
estimation scheme at (h,(/3) = (/,0) is almost global 
over the state space TSE(3) ~ SE(3) x R®, which is 
the best possible with continuous control and navigation 
schemes for systems evolving on a non-contractible state 
space [am' In the presence of measurement noise with 
bounded frequencies and amplitudes, one can show that 
the expected values of the state estimates converge to 
a bounded neighborhood of the true states. The size of 
this neighborhood, which can be considered as a measure 
of the robustness of this estimation scheme, depends on 
the values of the estimator gains J, W and D. These 
estimator gains can be selected based on balancing the 
transient and steady-state behavior of the estimator. 

Remark 4.2 In the special case that the weight matrix 
W in Wahba’s function is chosen as a piecewise time 

rp 

constant matrix according to Lemma 3.2, K = DWD^ 
is a constant matrix for all time. Therefore, the RHS of 
(46) is not explicitly dependent on time. This makes (h, (/j) 
an autonomous system and therefore the use of Theorem 

8.4 of fldil is not required to prove asymptotic stability. 
One can apply LaSalle’s invariance principle (Theorem 

4.4 in mn to prove the convergence of state estimates 
to the equilibrium (/, 0) in this case. 

5 Discretization for Computer Implementation 

For onboard computer implementation, the varia¬ 
tional estimation scheme outlined above has to be dis¬ 
cretized. This discretization is carried out in the frame¬ 
work of discrete geometric mechanics, and the resulting 
discrete-time estimator is in the form of a Lie group vari¬ 
ational integrator (LGVI), as in [S^. Since the estima¬ 
tion scheme proposed here is obtained from a variational 
principle of mechanics, it can be discretized by applying 
the discrete Lagrange-d’Alembert principle [l^. Con¬ 
sider an interval of time [to,T] G R+ separated into N 
equal-length subintervals \ti,ti+i] for i = 0,1,..., A^, 
with t]\[ = T and — U = At is the time step size. 
Let (giyfi) G SE(3) X R® denote the discrete state esti¬ 
mate at time ti, such that ss (g(ti), ^(ti)) where 
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is the exact solution of the continuous-time 
estimator at time t € [toiT']- Let the values of the 
discrete-time measurements a™ and L™ at time ti 
be denoted as a™ and L™, respectively. Further, 
denote the corresponding values for the latter two quan¬ 
tities in inertial frame at time ti by pi and Di, respec¬ 
tively. The term representing the energy content of the 
pose estimation error, given by (16), is discretized as 




A,ar,K)=W.(g*,Lr,A)+Z^t(gz,ar,Pz) 
= $(A°(g., Lr, A)) + A(g., ar,K) 

= $(i(A - R.LT, (A - R.LT)W,)) 

+ - Aa™ - Sjf, (63) 


where Fi G SO(3), (g(to)A(io)) = (go,lo), ‘Pi = 
[ujlf vlf] and S'ri(A) is the value of Sr{R) at time ti, 
with Sr{R) as defined by (34). 

Proof: Consider first variations with fixed endpoints for 
the pose estimation errors in discrete time given by: 

5Q, = Q,Sf, So = Sjv = 0, (71) 

Sxi = Q^p^, po = = 0, (72) 

where T,i, pi G are “discrete variation vectors”. It can 
be shown that for any w G we have 

J + Juj^. (73) 


where Wi is the matrix of weight factors corresponding 
to Di at time U. The term encapsulating the energy in 
the velocities estimate error (17), is discretized as 


Discretizing (12) assuming that the angular velocity es¬ 
timation error is constant in the time interval \ti,ti^i\ 
with a constant time step size At, one gets 


where J = diag(J,M) and M,J are positive definite 
matrices. 

Lemma 5.1 In the absence of measurement noise, the 
discrete-time Lagrangian is of the form 

£{h„D„p„ipi) = ^(Jwf ) -b ^{Mvi,Vi) (65) 

- <^{{1 -Q^,Ki)) - ^Ky^Vi, 

where yi = y{hi,pi) = Qfxi -b (/ - Qj')pi and J is 
defined in terms of the matrix J by J = ltrace[J]/ — J. 

A Lie group variational integrator (LGVl) introduced in 
is applied to the discrete-time Lagrangian (65) to 
obtain the discrete-time filter. 


Q,+i=Q,F,, iG{0,l,2,...,A-l}, (74) 

where Fi G SO(3) is given by 

Fi = exp(Atwf) « J -b I^tojf. (75) 

The variation of Fi can be derived from (74) and 5Qi = 
Qi£f. Thus 

(5F, = + (76) 

Using (73) and (75), one can enforce the skew-symmetry 
of {JuJiY by 

(= wf A + Accf « ^ ((A - I)J - J{f7 - I)) 

= ^^F^J-JF7)■ (A) 


Theorem 5.2 A first-order discretization of the estima¬ 
tor proposed in Theorem 3.3 is given by 


=7^^(RJ-JfT), (66) 

(M + A®t)r;,+i = F^Mv, (67) 

-b AtK(6i_|_i -b RiJ^iaIf\_i — Pi+i), 

(T -b Atl])j.)cu 2 _|_i = F^JtOi -b AtALuj^i x ViJ^\ 

+/Atnpf^.^{bi+iF Ri+id"ffi) (68) 

-At$'(A°(g.+i ,LT+^, ))(A+i), 

e.=C-Adg-i:p., (69) 

g,+i = g,exp(At|)^), (70) 


From (11), the continuous rate of change of the attitude 
estimation error is i = Qv, which can be approximated 
to first order in discrete-time as 


~ Qi^i — ^tC^iVi -b Xi- (^^) 

The first variation in Vi is then calculated using (78) as 

Ai = 5(^^Qj{xi+i -x,7) 

= -£fvi -b ■^^Qj{5xi+i - 5xi) 

= ~'F‘i 'Vi + “^APi+1 ITlPi' (A) 
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The discrete Lagrangian (65) can be rewritten as 

-/), (F, -/)) 

+ - At$(W°(hi, A)) (80) 

^ l^iQiVi) {.QiHi)- 


Separating this equation into two (rotational and trans¬ 
lational) parts leads to 

(M -I- AtD4)r;i+i = Mvi - Mnyi+i, (85) 

(J -I- AtDj.)wi+i = Juji + AtMvi+i X Ui+i 

- AtKpf_^j^y,+i (86) 

- At$'(A°(h,+i,A+i))5if,^i(Q*+i), 


The action functional (19) is replaced by the action sum 


N-l 

Sd{C{hi,Di,pi,Fi,Vi)) = At'^ jC{hi,Di,pi,Fi,Vi). 

i=0 

Applying the discrete Lagrange-d’Alembert principle 
with two Rayleigh dissipation terms for angular and 
translational motions gives 


SSd{C{hi,Di,pi,Fi,Vi)) (82) 

N-l 

+ X] = 0 

i=0 

N-l f ^ 

\ ■^^{SF^,J{F,-I)) + At{Sv,,Mv,) 

i^O [ ^ 

-^d>'(^°(h.,A))(Sr,^^,(Q0)-Ate(p,,y,) 
-AtK{Y,f ,yiPj) + ^{S^,Tl') + At{piJ,)^ = 0 . 

As symmetric matrices are orthogonal to skew- 
symmetric matrices in the trace inner product, using 
(75) we can rewrite the first term in (80) as 

{SF,,J{F, - I)) = (Sf, (83) 


using the identity F = [f "^and by replac¬ 

ing Ti = —Dj-Wi and fi = —DjUj, where and Dt are 
positive definite matrices such that 



_0 

rp 

In the presence of measurement noise, Q * Di and yi are 
replaced by RiL”^ andp —6 —Aa”*, respectively. These 
give the discrete-time state estimator in the form of the 
Lie group variational integrator (66)-(70). □ 

Model-based discrete-time rigid body state estimators 
using LGVI schemes for attitude estimation were re¬ 
ported in [10,El, but dynamics model-free state estima¬ 
tors using LG Vis have appeared only recently in [T0 . [T^ . 

Remark 5.3 In the absence of any direct velocity mea¬ 
surements or only angular velocity measurements, the ex¬ 
pressions provided in Section 3.3 to calculate rigid body 
velocities are still valid in discrete-time. One can use the 
discrete-time variables introduced in this section in place 
of their continuous-time counterparts. The second-order 
Butterworth filter (49) is discretized using the Newmark- 
P Method as follows: 



Choosing ujn = 2 and p, = i, this method gives the filtered 
positions and velocities as follows: 


Hence equation (82) can be re-expressed as 

^-1 r 1 1 

^ - -(Ef, (Ju;.)^) + -(E>Vi,fT(Jc,0"F,) 

2^0 I 

- X MviY) + {FiPi+i,Mvi) 

- {p.,MvP - ^<l>'(A°(h., A))(Er,F^^(Q.)> 
nAt 


-KAt{pi,y,) - —{Ef,{pfy^)^) 

+ ^(Sr,r,x) + At(p„/,)|=0. 


(84) 


^i+i 

M+i. 


1 


4 -I- ApuinAt F w^At^ 


4 -I- ApujnAt — ojfAf^ 
-4uj^At 


AAt 


w; 


( 88 ) 


lAP 


4 — ipujnAt — uxfAt^ 2uj^At 

J 


■J 


-I ~rn 

+ A-l-1 


. 


r 

where z'fi and zf are the corresponding value of quantities 
z'^ and z^ at time instant ti, respectively. As with the 
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are measured by the inertial sensors on the vehicle: 



Fig. 2. Position and attitude trajectory of the simulated 
vehicle in 3D space. 

r 

continuous time version, can be replaeed with in 
the estimator equations. 


6 Numerical Simulations 

This section presents numerical simulation results 
for the discrete-time estimator obtained in Section 
5. In order to numerically simulate this estimator, 
simulated true states of an aerial vehicle flying in a 
room are produced using the kinematics and dynam¬ 
ics equations of a rigid body. The vehicle mass and 
moment of inertia are taken to be = 420 g and 
Jv = [51.2 60.2 59.6]"^ g-m^, respectively. The resul¬ 
tant external forces and torques applied on the vehicle 
a,re(j)v{t) = 10“^[10cos(O.lt) 2sin(0.2t) — 2sin(0.5t)]"*" 
N and Tv{t) = N.m, respectively. The room 

is assumed to be a cubic space of size lOmxlOmxlOm 
with the inertial frame origin at the center of this cube. 
The initial attitude and position of the vehicle are: 

i?o = expmso(3) X [^ 

and bo = [2.5 0.5 - 3]'^ m. (89) 

This vehicle’s initial angular and translational velocity 
respectively, are: 

Oo = [0.2 - 0.05 0.1]'^ rad/s, 
and i/Q = [—0.05 0.15 0.03]"*" m/s. 


di = [0 0 - 1]'^, d2 = [0.1 0.975 - 0.2]'^ 


(91) 


For optical measurements, eight beacons are located at 
the eight vertices of the cube, labeled 1 to 8. The posi¬ 
tions of these beacons are known in the inertial frame 
and their index (label) and relative positions are mea¬ 
sured by optical sensors onboard the vehicle whenever 
the beacons come into the field of view of the sensors. 
Three identical cameras (optical sensors) and inertial 
sensors are assumed to be installed on the vehicle. The 
cameras are fixed to known positions on the vehicle, on 
a hypothetical horizontal plane passing through the ve¬ 
hicle, 120° apart from each other, as shown in Fig. 1. All 
the camera readings contain random zero mean signals 
whose probability distributions are normalized bump 
functions with width of 0.001m. The following are se¬ 
lected for the positive definite estimator gain matrices: 

J = diag([0.9 0.6 0.3]), 

M = diag([0.0608 0.0486 0.0365]), (92) 

=diag([2.7 2.2 1.5]),]D)t = diag([0.1 0.12 0.14]). 


<!)(•) could be any function with the properties de¬ 
scribed in Section 3, but is selected to be $(a;) = x here. 
The initial state estimates have the following values: 


go = /, Clo = [0.1 0.45 0.05]'^ rad/s, 
and i>o = [2.05 0.64 1.29]^^ m/s. 


(93) 


The performance of the proposed estimator is presented 
for two different cases. 


6.1 CASE 1: At least three beacons are observed at each 
time instant 

Having three beacons measured at each time instant 
guarantees full determination of vehicle’s translational 
and angular velocities instantaneously. A conic field of 
view (FOV) of 2x40° for cameras can satisfy this condi¬ 
tion. The vehicle’s velocity is calculated by (54) in this 
case. The discrete-time estimator (66)-(70) is simulated 
over a time interval of T = 20 s with sampling interval 
At = 0.02 s. At each time instant, (66) is solved using the 
Newton-Raphson iterative method to find an approxi¬ 
mation for Fi. Following this, the remaining equations 
(all explicit) are solved to generate the estimated states. 
The principal angle of the attitude estimation error and 
the position estimation error for CASE 1 are plotted in 
Fig. 3. Plots of the angular and translational velocity 
estimation errors are shown in Fig. 4. 


The vehicle dynamics is simulated over a time interval of 
T = 150 s, with a time stepsize of At = 0.02 s. The tra¬ 
jectory of the vehicle over this time interval is depicted 
in Fig. 2. The following two inertial directions, corre¬ 
sponding to nadir and Earth’s magnetic field direction. 


6.2 CASE 2: Less than three beacons are measured at 
some time instants 

To implement the variational estimator for the case 
that less than three optical measurements are available, 
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t (s) 


Fig. 3. Principal angle of the attitude and position estimation 
error for CASE 1. 





t (s) 


Fig. 5. Principal angle of the attitude and position estimation 
error for CASE 2. 




t (s) 


Fig. 4. Angular and translational velocity estimation error 
for CASE 1. 

the field of view of the cameras is decreased to limit 
the number of beacons observed. Assuming the cameras 
have conical fields of view of 2x25°, the minimum num¬ 
ber of beacons observed instantaneously drops to 1 dur¬ 
ing the simulated time interval. The dynamics model for 
the aerial vehicle, simulated time duration, and sample 
rate are identical to CASE 1. Fig. 5 depicts the princi¬ 
pal angle of the attitude estimation error and the posi¬ 
tion estimation error for CASE 2, and Fig. 6 shows the 
angular and translational velocity estimation errors. All 
estimation errors are shown to converge to a neighbor¬ 
hood of (h,(/3) = (1,0) in both cases, where the size of 
this neighborhood depends on the magnitude of mea¬ 
surement noise. 


7 Conclusion 

This article proposes an estimator for rigid body pose 
and velocities, using optical and inertial measurements 
by sensors onboard the rigid body. The sensors are as¬ 
sumed to provide measurements in continuous-time or 



Fig. 6. Angular and translational velocity estimation error 
for CASE 2. 


at a sufficiently high frequency, with bounded measure¬ 
ment noise. An artificial kinetic energy quadratic in rigid 
body velocity estimate errors is defined, as well as two 
fictitious potential energies: (1) a generalized Wahba’s 
cost function for attitude estimation error in the form of 
a Morse function, and (2) a quadratic function of the ve¬ 
hicle’s position estimate error. Applying the Lagrange- 
d’Alembert principle on a Lagrangian consisting of these 
energy-like terms and a dissipation term linear in veloc¬ 
ities estimation error, an estimator is designed on the 
Lie group of rigid body motions. In the absence of mea¬ 
surement noise, this estimator is shown to be almost 
globally asymptotically stable, with estimates converg¬ 
ing to actual states in a domain of attraction that is 
open and dense in the state space. The continuous esti¬ 
mator is discretized by applying the discrete Lagrange- 
d’Alembert principle on the discrete Lagrangian and dis¬ 
sipation terms linear in rotational and translational ve¬ 
locity estimation errors. In the presence of measurement 
noise, numerical simulations show that state estimates 
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converge to a bounded neighborhood of the true states. 
Future extensions of this work include higher-order dis¬ 
cretizations of the continuous-time filter given here and 
obtaining a stochastic interpretation of the variational 
pose estimator. 
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